
import ROSinfo
import rospy
import math
import threading
import ADRC_reward
from enu_xyz import GNC_COORDINATE
from queue import Queue
import matplotlib.pyplot as plt
import ADRC
import queue

from gazebo_msgs.srv import SetModelState
from gazebo_msgs.msg import ModelState
# 在全局范围内创建一个队列
boat_pos_queue = Queue()
boat_yaw_queue = Queue()
boat_vel_queue = Queue()
boat_speed_queue = Queue()

def goto_point(given_point):
    # 等待服务可用
    rospy.wait_for_service('/gazebo/set_model_state')
    try:
        # 创建服务代理
        set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)

        # 构造状态消息
        state_msg = ModelState()
        state_msg.model_name = 'wamv'
        state_msg.pose.position.x = given_point[0]
        state_msg.pose.position.y = given_point[1]
        state_msg.pose.position.z = 0.0  # 假设 z 坐标为 0
        # 保持默认朝向（或者根据需要设置四元数）
        state_msg.pose.orientation.x = 0.0
        state_msg.pose.orientation.y = 0.0
        state_msg.pose.orientation.z = 0.0
        state_msg.pose.orientation.w = 1.0
        state_msg.reference_frame = 'world'

        # 调用服务进行状态设置（瞬移）
        resp = set_state(state_msg)
        print("无人船已瞬移到目标点：", given_point)
    except rospy.ServiceException as e:
        print("调用 /gazebo/set_model_state 服务失败: %s" % e)



yaw_list = []
dis_list = []
if __name__ == "__main__":

    gnc_coordinate = GNC_COORDINATE()
    # 训练路径航点
    points = [
        [150.6755737, -33.7221389],  # 第一个坐标点
        [150.6825302, -33.7221389],  # 第二个坐标点
    ]
    # 设置初始点经纬度
    origin_lon = points[0][0]
    origin_lat = points[0][1]
    #写一个for循环，将每个点的经纬度转换为坐标系
    for i in range(len(points)):
        points[i][0] = gnc_coordinate.Lon_trans_to_E(points[i][0], points[i][1], origin_lon, origin_lat)
        points[i][1] = gnc_coordinate.Lat_trans_to_N(points[i][0], points[i][1], origin_lon, origin_lat)


    def update_func_gps(handle, lat, lon, alt):
        # 将ROS传回的经纬度转换为坐标系
        boat_x = gnc_coordinate.Lon_trans_to_E(lon, lat, origin_lon, origin_lat)
        boat_y = gnc_coordinate.Lat_trans_to_N(lon, lat, origin_lon, origin_lat)
        boat_pos_queue.put((boat_x, boat_y))


    def update_func_ea(handle, yaw, pitch, roll):
        boat_yaw_queue.put(yaw)

    def update_func_ground_speed(handle, speed , forward_v , lateral_v):
        boat_vel_queue.put((forward_v,lateral_v))
        boat_speed_queue.put(speed)

    def update_func_gyro(handle, gyroX, gyroY, gyroZ):
        pass

    def update_func_acc(handle, accX, accY, accZ):
        pass

    rospy.init_node('my_ROSVRX_node')

    update_handle = None

    ros_vrx_info = ROSinfo.ROSVRXInfo(update_func_gps, update_func_ea, update_func_ground_speed,
                              update_func_gyro, update_func_acc, update_handle)

    propeller_mid = 0
    servo_mid = 0
    rosvrx_control = ROSinfo.propulsion_VRX(propeller_mid, servo_mid)
    ADRC_param = ADRC.LADRC_NUM(2, 200, 200/4, 1500)  # 调整 w0 和 wc 试验

    ADRC_ctl = ADRC.LADRC(ADRC_param)
    k=1
    def publish_commands():
        origin_point = [-350, 232]  # 初始位置
        goto_point(origin_point)
        while 1:

            try:
                boat_x, boat_y = boat_pos_queue.get_nowait()
                yaw = boat_yaw_queue.get_nowait()
                speed = boat_speed_queue.get_nowait()
            except queue.Empty:
                continue  # 如果数据还没来，跳过这次循环
            if ((boat_x - points[k][0]) ** 2 + (boat_y - points[k][1]) ** 2) < 500*500:
                print('end')
                break
            else:
                dis, exceped_ang =ADRC_reward.calculate_metrics(yaw, 0, 0, points[k-1][0], points[k-1][1], points[k][0], points[k][1], boat_x, boat_y)
                ADRC_ctl.LADRC_Loop(0,exceped_ang)
                turn = ADRC_param.u
                power_R = 0.8
                power_L = 0.8
                rosvrx_control.manual_act(power_L, power_R, turn)
                print(ADRC_param.u,ADRC_param.v1,ADRC_param.v2,ADRC_param.z1,ADRC_param.z2,ADRC_param.z3)
                distance = math.sqrt((boat_x - points[k][0]) ** 2 + (boat_y - points[k][1]) ** 2)
                # print(points[k-1][0], points[k-1][1], points[k][0], points[k][1])
                print('当前位置：', boat_x, boat_y, '当前航向：', yaw, '期望航向:', exceped_ang, '当前速度：', speed, '当前距离：', distance)

                #创建一个数组将yaw存下来
                yaw_list.append(exceped_ang * 180 / math.pi)
                dis_list.append(dis)
        #画出yaw_list的图像
        plt.plot(yaw_list)
        plt.plot(dis_list)
        plt.show()  # 在主线程中显示图形
    # 创建一个线程来发布命令
    publish_thread = threading.Thread(target=publish_commands)
    publish_thread.start()

    try:
        ros_vrx_info.ROSVRX_info_create()

    except rospy.ROSInterruptException:
        pass
